﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using RoardersAgentSim.Map;
using RoardersAgentSim.Agent;

namespace RoardersAgentSim
{
	public class Algorithms
	{
        /// <summary>
        /// Compute shortest paths and return the shortest path 
        /// </summary>
        /// <param name="map"></param>
        /// <param name="startNode"></param>
        /// <returns></returns>
        public static Dictionary<Node,LinkPath> ComputeShortestPath(FullMap map, Node startNode,LinkType linkType)
        {
            Dictionary<Node, LinkPath> paths = new Dictionary<Node, LinkPath>();
            Dictionary<string, string> P = new Dictionary<string, string>();
            Dictionary<string, double> d = new Dictionary<string, double>();
            foreach (Node node in map.Nodes)
            {
                d.Add(node.NodeID, double.PositiveInfinity);
                P.Add(node.NodeID, null);
            }
            d[startNode.NodeID] = 0;
            //multimea de varfuri deja vizitate
            List<string> selected = new List<string>();
            selected.Add(startNode.NodeID);
            //(o coada cu prioritati, in care nodurile se afla ordonate dupa distanta fata de sursa) din care este mereu extras nodul aflat la distanta minima.
            List<string> Q = new List<string>();

            foreach (Node n in map.Nodes)
            {
                if (n.NodeID == startNode.NodeID)
                    continue;
                if (startNode.IsDirectionalNeighborWith(n,linkType))
                {
                    RoadLink rl = startNode.GetConnectingLink(n,linkType);
                    if (rl != null)
                    {
                        d[n.NodeID] = rl.Link.Length;
                        Q.Add(n.NodeID);
                        P[n.NodeID] = startNode.NodeID;
                    }
                    else
                    {
                        throw new Exception("Something went wrong - > the node was found as neighbor but no link was returned");
                    }
                }
                else
                {
                    d[n.NodeID] = double.PositiveInfinity;
                    P[n.NodeID] = null;
                }
            }

            while (Q.Any())
            {
                string u = Q[0];
                double min = d[u];
                foreach (string s in Q)
                {
                    if (min > d[s])
                    {
                        min = d[s];
                        u = s;
                    }
                }
                selected.Add(u);
                Q.Remove(u);
                Node startingNode = map.GetNode(u);
                var neighbors = map.Nodes.Where(x => startingNode.IsDirectionalNeighborWith(x,linkType));
                if(neighbors != null)
                {
                    foreach(Node nod in neighbors)
                    {
                        RoadLink rl = startingNode.GetConnectingLink(nod,linkType);
                        if(!selected.Any(x=>x == nod.NodeID) && rl!=null && (d[nod.NodeID] == double.PositiveInfinity || d[nod.NodeID] > d[u] + rl.Link.Length))
                        {
                            d[nod.NodeID] = d[u] + rl.Link.Length;
                            P[nod.NodeID] = u;
                            if (!Q.Contains(nod.NodeID))
                                Q.Add(nod.NodeID);
                            else
                                continue;
                                //throw new Exception("Something went wrong on the algorithm");
                        }
                    }
                }
            }
            LinkPath pathList = new LinkPath();
            foreach(Node n in map.Nodes.Where(x=>x!=startNode))
            {
                string nod = P[n.NodeID];
                if (nod == null)
                    continue;
                Node temp = map.GetNode(nod);
                RoadLink rl = temp.GetConnectingLink(n,linkType);
                while (!string.IsNullOrEmpty(nod))
                {
                    pathList.RoadLinks.AddFirst(rl);
                    string previous = nod;
                    nod = P[nod];
                    if (nod != null)
                    {
                        temp = map.GetNode(nod);
                        rl = temp.GetConnectingLink(map.GetNode(previous),linkType);
                    }
                }
                paths.Add(n, pathList);
                pathList = new LinkPath();
            }
            return paths;
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="linkType"></param>
        /// <param name="startNode"></param>
        /// <param name="destination"></param>
        /// <param name="population"></param>
        /// <param name="fullMap"></param>
        /// <returns></returns>
        internal static LinkPath CalculateOptimalCostPath(LinkType linkType, Node startNode, Shelter destination, Population population, FullMap fullMap)
        {
            if (startNode == destination.Node)
                return null;
            LinkPath chosen = new LinkPath() ;
            Dictionary<Node, LinkPath> paths = new Dictionary<Node, LinkPath>();
            Dictionary<string, string> P = new Dictionary<string, string>();
            Dictionary<string, double> d = new Dictionary<string, double>();
            foreach (Node node in fullMap.Nodes)
            {
                d.Add(node.NodeID, double.PositiveInfinity);
                P.Add(node.NodeID, null);
            }
            d[startNode.NodeID] = 0;
            //multimea de varfuri deja vizitate
            List<string> selected = new List<string>();
            selected.Add(startNode.NodeID);
            //(o coada cu prioritati, in care nodurile se afla ordonate dupa distanta fata de sursa) din care este mereu extras nodul aflat la distanta minima.
            List<string> Q = new List<string>();

            foreach (Node n in fullMap.Nodes)
            {
                if (n.NodeID == startNode.NodeID)
                    continue;
                if (startNode.IsDirectionalNeighborWith(n,linkType))
                {
                    RoadLink rl = startNode.GetConnectingLink(n, linkType);
                    if (rl != null)
                    {
                        double cost = population.GetZetaBar(rl) + (population.eta!=rl.rho? Utils.GetLargeM():0);
                        d[n.NodeID] = rl.CalculateCost(cost);
                        Q.Add(n.NodeID);
                        P[n.NodeID] = startNode.NodeID;
                    }
                    else
                    {
                        throw new Exception("Something went wrong - > the node was found as neighbor but no link was returned");
                    }
                }
                else
                {
                    d[n.NodeID] = double.PositiveInfinity;
                    P[n.NodeID] = null;
                }
            }

            while (Q.Any())
            {
                string u = Q[0];
                double min = d[u];
                foreach (string s in Q)
                {
                    if (min > d[s])
                    {
                        min = d[s];
                        u = s;
                    }
                }
                selected.Add(u);
                Q.Remove(u);
                Node startingNode = fullMap.GetNode(u);
                var neighbors = fullMap.Nodes.Where(x => startingNode.IsDirectionalNeighborWith(x,linkType));
                if (neighbors != null)
                {
                    foreach (Node nod in neighbors)
                    {
                        RoadLink rl = startingNode.GetConnectingLink(nod,linkType);
                        double cost = population.GetZetaBar(rl) + (population.eta != rl.rho ? Utils.GetLargeM() : 0);

                        if (!selected.Any(x => x == nod.NodeID) && rl != null && (d[nod.NodeID] == double.PositiveInfinity || d[nod.NodeID] > d[u] + rl.CalculateCost(cost)))
                        {
                            d[nod.NodeID] = d[u] + rl.CalculateCost(cost);
                            P[nod.NodeID] = u;
                            if (!Q.Contains(nod.NodeID))
                                Q.Add(nod.NodeID);
                            else
                                continue;
                                //throw new Exception("Something went wrong on the algorithm");
                        }
                    }
                }
            }
            string nn = P[destination.Node.NodeID];
            Node temp = fullMap.GetNode(nn);
            RoadLink rlTemp = temp.GetConnectingLink(destination.Node);
            while (!string.IsNullOrEmpty(nn))
            {
                chosen.RoadLinks.AddFirst(rlTemp);
                string previous = nn;
                nn = P[nn];
                if (nn != null)
                {
                    temp = fullMap.GetNode(nn);
                    rlTemp = temp.GetConnectingLink(fullMap.GetNode(previous));
                }
            }
            return chosen;
        }
    }
}
